#include "bevdet_tracker.h"

#include <rclcpp/rclcpp.hpp>

std::string debug_message(bevdet::tracker::ObjectSptrVector objects)
{
    std::stringstream ss;
    ss << "tracked objects num:" << objects.size();
    for (auto& obj : objects) {
        ss << " " << obj->id;
    }

    return ss.str();
}

int main(int argc, char* argv[])
{
    auto logger = rclcpp::get_logger("perception");
    RCLCPP_INFO(logger, "perception start!");

    // Force flush of the stdout buffer.
    setvbuf(stdout, NULL, _IONBF, BUFSIZ);

    rclcpp::init(argc, argv);

    auto node = std::make_shared<rclcpp::Node>("demo");

    bevdet::tracker::BevdetTracker tracker;
    auto sub_ = node->create_subscription<interface::msg::Objects>("/perception/multiFused/objects", 10,
        [&](const interface::msg::Objects::SharedPtr objects) {
            int64_t us = int64_t(objects->header.stamp.sec) * 1000 * 1000 + objects->header.stamp.nanosec / 1000;
            bevdet::tracker::ObjectSptrVector objs;
            for (auto& obj : objects->objects) {
                objs.push_back(std::make_shared<interface::msg::Object>(obj));
            }
            RCLCPP_INFO(logger, "%s", debug_message(tracker.update(us, objs)).c_str());
        });

    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}